// ======================================================================
// Copyright (C) 2013 Hell-Prototypes. / www.hellprototypes.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
// ======================================================================
#include "hell_arm.h"
#include "ui_hell_arm.h"

/*
bool Hell_Arm::adjusting_part_orient(double target_orient)
{
    if(!set_picker_angle(target_orient)) {
        return false;
    }

    return true;
}
*/
bool Hell_Arm::get_part_feeder_zone_center(iPoint &zone_center)
{
    QString value;
    if(!get_current_part_value(value)) {
        disp_system_msg("Fail to get current part value item");
        return false;
    }
    if(value.isEmpty()) {
        disp_system_msg("Part's value item was empty");
        return false;
    }

    QList <int> keys = m_ctrl_feed_zone_map.keys();
    ctrl_feed_zone *p_ctrl_feed_zone;
    for(int i=0; i<keys.size(); i++) {
        p_ctrl_feed_zone = m_ctrl_feed_zone_map[keys.at(i)];
        QString feed_value = p_ctrl_feed_zone->get_selected_value();
        if(feed_value == value) {
            zone_center = p_ctrl_feed_zone->get_feeder_zone_position();
            QSize size = p_ctrl_feed_zone->get_feeder_zone_size();
            m_vision_thread->set_feeder_zone_size(size);
            m_vision_thread->set_part_area(p_ctrl_feed_zone->get_feeder_part_area());
            return true;
        }
    }
    return false;
}

void Hell_Arm::on_pb_move_arm_to_origin_clicked()
{
    m_vision_thread->stop_capture_parts_position();
    m_pap_thread->set_picker_angle(PICKER_IDLE_ANGLE);
    if(!m_pap_thread->move_arm_to_origin()) {
        disp_system_msg("Move arm to origin FAIL");
        return;
    }
    msdelay(500);

    if(m_origin_zone_size.isValid()) {
        m_vision_thread->set_feeder_zone_size(m_origin_zone_size);
    }
    m_vision_thread->set_part_area(m_origin_zone_area);

    m_exe_origin_auto_correct = true;
    m_vision_thread->capture_parts_position();
    m_vision_thread->start();
}

void Hell_Arm::origin_auto_correct(const Point &center_pos)
{
    QSize img_size = m_vision_thread->get_image_size();
    int img_center_x = img_size.width()/2;
    int img_center_y = img_size.height()/2;

    double K = mm2mil(ui->dsb_cam_width_length->value())/(img_center_x*2 + 1e-10);
    int x_offset = (center_pos.x - img_center_x) * K / ONE_STEP_DISTANCE;
    int y_offset = (center_pos.y - img_center_y) * K / ONE_STEP_DISTANCE;

    if((abs(x_offset) < 8) && (abs(y_offset) < 8)) {
        m_pap_thread->m_curr_arm_pos.x = 0;
        m_pap_thread->m_curr_arm_pos.y = 0;
        m_exe_origin_auto_correct = false;
        disp_system_msg("Origin auto correct DONE");
        return;
    }

    iPoint target;
    target.x = m_pap_thread->m_curr_arm_pos.x + x_offset;
    target.y = m_pap_thread->m_curr_arm_pos.y - y_offset;

    m_pap_thread->set_picker_angle(PICKER_IDLE_ANGLE);
    if(!m_pap_thread->move_arm_to(target, 1000)) {
        disp_system_msg("Move arm to correct position FAIL");
        return;
    }
    msdelay(500);

    m_vision_thread->capture_parts_position();
    m_vision_thread->start();
}

void Hell_Arm::slot_parts_position_info_process(vector<part_t> *parts_pos)
{
    part_t part, closest_part;
    DPoint place_coor;
    double target_orient = -1;
    double minimum;
    QSize img_size = m_vision_thread->get_image_size();

    if(parts_pos->size() <= 0) {
        disp_system_msg("No part found in parts area");
        slot_pap_error_process(-3);
        return;
    }

    //Found the part close to image centre
    closest_part.angle = parts_pos->at(0).angle;
    closest_part.center = parts_pos->at(0).center;
    minimum = m_pap_thread->distance_of_2_point(closest_part.center.x, closest_part.center.y,
                                                img_size.width()/2, img_size.height()/2);
    for(uint i=1; i<parts_pos->size(); i++) {
        part.angle = parts_pos->at(i).angle;
        part.center = parts_pos->at(i).center;
        double dist = m_pap_thread->distance_of_2_point(part.center.x, part.center.y,
                                                        img_size.width()/2, img_size.height()/2);
        if(dist < minimum) {
            closest_part.angle = part.angle;
            closest_part.center = part.center;
            minimum = dist;
        }
    }

    if(m_exe_origin_auto_correct) {
        origin_auto_correct(closest_part.center);
        return;
    }

//    Move camera to part first before pick up
//    QSize img_size = m_vision_thread->get_image_size();
//    if(m_pap_thread->distance_of_2_point(part.center.x, part.center.y,
//                                         img_size.width()/2, img_size.height()/2) > 10 ) {
//        if(!m_pap_thread->move_camera_by_offset(x, y)) {
//            disp_system_msg("Fail move camera to part place");
//        }
//        return;
//    }

    if(!ui->cb_only_pickup->isChecked()) {
        if(get_current_part_parameters(place_coor, target_orient) ){
            disp_system_msg(QString("Current Place part coordinate: X=%1, Y=%2, Orient.=%3").arg(place_coor.x).arg(place_coor.y).arg(target_orient*180/PI));
        } else {
            disp_system_msg("Fail to get part coordinate in list");
            slot_pap_error_process(-4);
            return;
        }
    }

    m_feeder_have_parts = true;
    m_pap_thread->set_move_param(closest_part, place_coor, target_orient);
    m_pap_thread->start();
}

void Hell_Arm::prepare_for_auto_pap()
{
    static int s_auto_align_count = 0;

    int auto_align_ctrl_value = ui->sb_origin_auto_align_ctrl->value();

    if((auto_align_ctrl_value > 0) && (m_origin_align_flag == false) && m_feeder_have_parts == true) {
        if(s_auto_align_count >= auto_align_ctrl_value) {
            s_auto_align_count = 0;
            m_origin_align_flag = true;
            on_pb_move_arm_to_origin_clicked();
            return;
        }
    }

    if(auto_align_ctrl_value > 0) {
        s_auto_align_count++;
    }
    m_origin_align_flag = false;

    on_pb_manual_pick_and_place_clicked();
}


void Hell_Arm::on_pb_manual_pick_and_place_clicked()
{
    iPoint zone_center;
    if(!get_part_feeder_zone_center(zone_center)) {
        slot_pap_error_process(-1);
        disp_system_msg("Fail to get feeder zone param ...");
        return;
    }

    m_pap_thread->set_picker_angle(PICKER_IDLE_ANGLE);
    if(!m_pap_thread->move_cam_to_feeder_zone(zone_center)) {
        slot_pap_error_process(-2);
        disp_system_msg("Move arm to feeder zone FAIL");
        return;
    }
    msdelay(100);

    if(!m_feeder_have_parts) {
        QApplication::beep();
        disp_system_msg("Warning: No part found in current feeder zone");
    }

    m_feeder_have_parts = false;
    m_vision_thread->capture_parts_position();
    m_vision_thread->start();

    //receive parts position in "slot_parts_position_info_process function"
}

void Hell_Arm::slot_auto_pap_timer_timeout()
{
    if(ui->pb_auto_pick_and_place->isChecked()) {
        if((!m_pap_thread->isRunning()) && (m_exe_origin_auto_correct == false)) {
            prepare_for_auto_pap();
            m_auto_pap_timer.start(1500);//FIXME: this may make over run
        } else {
            m_auto_pap_timer.start(100);
        }
    }
}

void Hell_Arm::slot_pap_error_process(int error_code)
{
    if((error_code == -1) && (ui->cb_skip_undefine_part_value->isChecked())) {
        DPoint curr_coordinate;
        if(get_current_part_coordinate(curr_coordinate)) {
            m_origin_align_flag = true;// do not align when undefine part getted
            return;
        }
    }

    QApplication::beep();
    ui->pb_auto_pick_and_place->setChecked(false);
    ui->pb_auto_pick_and_place->setStyleSheet("");
}

void Hell_Arm::on_pb_auto_pick_and_place_clicked(bool checked)
{
    if(checked) {
        m_auto_pap_timer.start(50);
        ui->pb_auto_pick_and_place->setStyleSheet("background-color:Black; color:Red");
    } else {
        ui->pb_auto_pick_and_place->setStyleSheet("");
        m_vision_thread->stop_capture_parts_position();
    }
}

void Hell_Arm::on_pb_arm_move_to_pcb_part_clicked()
{
    DPoint pcb_part_coor;
    iPoint target;

    if(get_current_part_coordinate(pcb_part_coor) ){
        disp_system_msg(QString("Part coordinate: x=%1, y=%2").arg(pcb_part_coor.x).arg(pcb_part_coor.y));
    } else {
        disp_system_msg("Fail to get part coordinate in list");
        return;
    }

    if(!m_pap_thread->place_coor_to_arm_arg(&pcb_part_coor, target)) {
        disp_system_msg("Fail to convert pcb coor to arm coor");
        return;
    }
    disp_system_msg(QString("Target XY: x=%1, y=%2").arg(target.x).arg(target.y));

    if(!m_pap_thread->picker_move_up()) {
        disp_system_msg("Fail to move picker up");
        return;
    }

    if(!m_pap_thread->move_arm_to(target, 1000)) {
        disp_system_msg("Fail move picker to target place");
        return;
    }
    if(!m_pap_thread->place_picker_move_down()) {
        disp_system_msg("Fail to move picker down");
        return;
    }
}
